#ifdef KITSCHY_DEBUG_MEMORY 
#include "debug_memorymanager.h"
#endif

#ifdef WIN32
#include "windows.h"
#endif

#include "stdio.h"
#include "stdlib.h"
#include "string.h"
#include "math.h"

#include <GL/gl.h>
#include <GL/glut.h>

#include "BList.h"
#include "BVector.h"
#include "geometrics.h"
#include "quaternion.h"
#include "symbol.h"
#include "sort.h"
#include "ontology.h"
#include "featureterm.h"
#include "FTKBase.h"
#include "SpecialTerm.h"
#include "ST_floatInterval.h"

#include "AWLocalViewport.h"

#include "AWEntity.h"
#include "AWMessage.h"
#include "AWScenario.h"
#include "AWBody.h"
#include "AWMoveBody.h"


AW_MoveBody::AW_MoveBody(Ontology *O,FTKBase *Memory) : AW_Body(O,Memory) 
{
	operators();

	class_name->set("AW_MoveBody");

	m_current_move=-1; 
	m_current_move_time_remaining=0;
} /* AW_MoveBody::AW_MoveBody */ 


AW_MoveBody::AW_MoveBody(FILE *fp,Ontology *O,FTKBase *Memory) : AW_Body(O,Memory)
{
	operators();

	class_name->set("AW_MoveBody");

	m_current_move=-1; 
	m_current_move_time_remaining=0;
	
	load(fp);
} /* AW_MoveBody::AW_MoveBody */ 


void AW_MoveBody::operators(void)
{
	FeatureTerm *op;

	m_ontology->new_sort("rotate-operator","any",2,"speed","float","time","float");
	op=FeatureTerm::fromStringNOOS("(define (rotate-operator))",m_memory,m_ontology);

	op->defineFeatureValue("speed",new FeatureTerm((Symbol *)0,
												   new ST_floatInterval(m_ontology->get_sort("float-interval"),
																		new FeatureTerm((Symbol *)0,-180.0F,m_ontology),
																		new FeatureTerm((Symbol *)0,180.0F,m_ontology)),
												   m_ontology->get_sort("float-interval")));
	op->defineFeatureValue("time",new FeatureTerm((Symbol *)0,
												   new ST_floatInterval(m_ontology->get_sort("float-interval"),
																		new FeatureTerm((Symbol *)0,0,m_ontology),
																		0),
												   m_ontology->get_sort("float-interval")));
	m_ops.Add(op);

	m_ontology->new_sort("advance-operator","any",2,"speed","float","time","float");
	op=FeatureTerm::fromStringNOOS("(define (advance-operator))",m_memory,m_ontology);
	op->defineFeatureValue("speed",new FeatureTerm((Symbol *)0,
												   new ST_floatInterval(m_ontology->get_sort("float-interval"),
																		new FeatureTerm((Symbol *)0,-10.0F,m_ontology),
																		new FeatureTerm((Symbol *)0,10.0F,m_ontology)),
												   m_ontology->get_sort("float-interval")));
	op->defineFeatureValue("time",new FeatureTerm((Symbol *)0,
												   new ST_floatInterval(m_ontology->get_sort("float-interval"),
																		new FeatureTerm((Symbol *)0,0,m_ontology),
																		0),
												   m_ontology->get_sort("float-interval")));
	m_ops.Add(op);
} /* AW_MoveBody::operators */ 



bool AW_MoveBody::load(FILE *fp)
{
	if (!AW_Body::load(fp)) return false;

	return true;
} /* AW_MoveBody::load */ 


bool AW_MoveBody::save(FILE *fp)
{
	if (!AW_Body::save(fp)) return false;

	return true;
} /* AW_MoveBody::save */ 


bool AW_MoveBody::is_a(char *c)
{
	if (strcmp(c,"AW_MoveBody")==0) return true;

	return AW_Body::is_a(c);
} /* AW_MoveBody::is_a */ 


bool AW_MoveBody::is_a(Symbol *c)
{
	if (c->cmp("AW_MoveBody")) return true;

	return AW_Body::is_a(c);
} /* AW_MoveBody::is_a */ 


bool AW_MoveBody::ApplyOperator(FeatureTerm *op,float time,AW_Agent *a,AW_World *w)
{
	if (strcmp(op->getSort()->get(),"rotate-operator")==0) {
		m_current_move=1;
		m_current_rotation=float((op->featureValueQuick("speed")->getFloat()*M_PI)/180);
		m_current_move_time_remaining=op->featureValueQuick("time")->getFloat();
		return true;
	} /* if */ 

	if (strcmp(op->getSort()->get(),"advance-operator")==0) {
		float v[4]={0,0,0,0},v2[4];
		v[2]=(op->featureValueQuick("speed")->getFloat());
		
		m_d.rotate_vector(v,v2);

		m_current_move=0;
		m_current_translation.x=v2[0];
		m_current_translation.y=v2[1];
		m_current_translation.z=v2[2];
		m_current_move_time_remaining=op->featureValueQuick("time")->getFloat();
		return true;
	} /* if */ 

	return AW_Body::ApplyOperator(op,time,a,w);
} /* AW_MoveBody::ApplyOperator */ 


void AW_MoveBody::actualize(float time,float timeinc,List<AW_Message> *message_queue)
{
	switch(m_current_move) {
	case 0: 
			{
				float op_time=0;
				
				if (m_current_move_time_remaining<timeinc) {
					op_time=m_current_move_time_remaining;
					m_current_move_time_remaining=0;
					m_current_move=-1;
				} else {
					op_time=timeinc;
					m_current_move_time_remaining-=op_time;
				} /* if */ 

				m_pos.x+=m_current_translation.x*op_time;
				m_pos.y+=m_current_translation.y*op_time;
				m_pos.z+=m_current_translation.z*op_time;

			}
			break;
	case 1:	{
				Quaternion q;
				float axis[3]={0,1,0};
				float op_time=0;
				
				if (m_current_move_time_remaining<timeinc) {
					op_time=m_current_move_time_remaining;
					m_current_move_time_remaining=0;
					m_current_move=-1;
				} else {
					op_time=timeinc;
					m_current_move_time_remaining-=op_time;
				} /* if */ 

				q.from_axis_angle(axis,m_current_rotation*op_time);
				m_d.pre_multiply(&q);
			}
			break;
	} /* switch */ 
} /* AW_MoveBody::actualize */ 
